#寻找交点
import sensor, image, time
from pyb import UART
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

clock = time.clock()

uart = UART(3,115200)


right_angle_threshold = (70, 110)
binary_threshold = [(0, 30)]


def calculate_angle(line1, line2):
# 利用四边形的角公式， 计算出直线夹角
    angle = (180 - abs(line1.theta() - line2.theta()))
    if angle > 90:
        angle = 180 - angle
    return angle


def is_right_angle(line1, line2):
    global right_angle_threshold
    # 判断两个直线之间的夹角是否为直角
    angle = calculate_angle(line1, line2)

    if angle >= right_angle_threshold[0] and angle <= right_angle_threshold[1]:
    # 判断在阈值范围内
        return True
    return False

def calculate_intersection(line1, line2):
    # 计算两条线的交点
    a1 = line1.y2() - line1.y1()
    b1 = line1.x1() - line1.x2()
    c1 = line1.x2()*line1.y1() - line1.x1()*line1.y2()

    a2 = line2.y2() - line2.y1()
    b2 = line2.x1() - line2.x2()
    c2 = line2.x2() * line2.y1() - line2.x1()*line2.y2()

    if (a1 * b2 - a2 * b1) != 0 and (a2 * b1 - a1 * b2) != 0:
        cross_x = int((b1*c2-b2*c1)/(a1*b2-a2*b1))
        cross_y = int((c1*a2-c2*a1)/(a1*b2-a2*b1))
        return (cross_x, cross_y)
    return (-1, -1)


def pack_test_data(x, y):

    pack_data=bytearray([x,y,0X00])

    lens = len(pack_data)#数据包大小

    i = 0
    sum = 0

    #和校验
    while i<(lens-1):
        sum = sum + pack_data[i]
        i = i+1
    pack_data[lens-1] = sum

    return pack_data


def find_verticle_lines(lines):
    line_num = len(lines)
    for i in range(line_num -1):
        for j in range(i, line_num):
            if is_right_angle(lines[i], lines[j]):
                return (lines[i], lines[j])
    return (None, None)


def uart_data_read():


        if uart.readchar() == 0xAA:
            uart.write(pack_test_data(cross_x, cross_y ))


while(True):
    clock.tick()
    img = sensor.snapshot()
    img.binary(binary_threshold)


    lines =  img.find_lines(threshold = 3000,x_stride = 6, y_stride = 6)

    for line in lines:
        pass
       # img.draw_line(line.line(), color = (255, 0, 0))
    # 如果画面中有两条直线

    if len(lines) >= 2:
        (line1, line2) = find_verticle_lines(lines)
        if (line1 == None or line2 == None):
            pass
        else:

            img.draw_line(line1.line(), color = (255, 0, 0))
            img.draw_line(line2.line(), color = (255, 0, 0))


            (cross_x, cross_y) = calculate_intersection(line1, line2)

    if uart.any():
        uart_data_read()









    
    









